'''
####
Date : 2025-1-8
Notes: 通讯
此脚本主要功能:
1. 新建线程与ZBot客户端进行通讯-数据交互
2. 监控相关寄存器值,做出指定动作。如:产品中的KT产线,台面的按钮（急停等）操作

####
'''

# -*- coding: utf-8 -*-
import logging
import os
import socket
import select
import struct
import time
import errno
import threading
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp

from threading import Thread
from PyQt5.QtCore import QTimer

# from processModbusTCP__ import process_Master
from processModbusRTU_ import rtu_Master
from processCommunicate_ import VSR_Server

# C++编写的ModbusTCP服务端
from TCPmodbus_server import Modbus_Server
# C++编写的ModbusTCP客户端
from TCPmodbus_wrapper import ModbusMaster
import config
### 日志文件
robot_logger=logging.getLogger("ZMove")
robot_logger.setLevel(logging.DEBUG)
### 默认显示
os.environ['DISPLAY'] = ':0'

# process_master = process_Master()
rtu_master = rtu_Master()

class Controler_Server():

    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)
        self.host = host
        self.port = port

        self.server_tcp_init()
        self.modbusClients = list()
        self.vsrserver = VSR_Server()
        self.server_th = Thread(target= self.vsrserver.run)
        self.server_th.start()

        self.tcpserver = IOA_Server()
        self.algo_th = Thread(target= self.tcpserver.run)
        self.algo_th.start() 


        self.th_update = Thread(target= self.status_update)
        self.th_update.start()
        
        # self.monitor_server_socket_thread = Thread(target= self.monitor_server_socket, daemon=False)
        # self.monitor_server_socket_thread.start()

    def server_tcp_init(self):  
        # self.server = modbus_tcp.TcpServer(port=self.port, address=self.host, timeout_in_sec=5)  
        # self.server.start()  
        # self.register_init()  
        self.server = Modbus_Server() 
        self.server_thre = threading.Thread(target = self.server.run)
        self.server_thre.start()

        self.modbus_master = ModbusMaster()

    def monitor_server_socket(self):
        while True:
            try:
                address = self.server.received_data
                if address is not None:
                    self.modbusClients.append(address) 
                time.sleep(0.01)
            except Exception as e:
                print(e)


        ## 用于追踪已连接的客户端地址
        # connected_clients = set()
        # while True:
            # ## 过滤掉无效的套接字

            # valid_sockets = [s for s in self.server._sockets if s.fileno() >= 0]  
            # try:
            #     inputready, _, _ = select.select(valid_sockets, [], [], 1.0)
            #     for sock in inputready:
            #         if sock == self.server._sock: 
            #             try:
            #                 client, address = sock.accept()
            #                 client.setblocking(0)
            #                 fileno = client.fileno()
            #                 addr_str = str(address)

            #                 ## 如果是新客户端，则打印连接信息并添加到已连接客户端集合中
            #                 if addr_str not in connected_clients:
            #                     connected_clients.add(addr_str)
            #                     self.modbusClients.append(address) 
            #                     self.server._sockets.append(client)

            #             except OSError as e:
            #                 ## 忽略特定的非阻塞操作错误 [Errno 11] Resource temporarily unavailable 或 [Errno 35] Resource temporarily unavailable
            #                 if e.errno != errno.EAGAIN and e.errno != errno.EWOULDBLOCK and e.errno != errno.EINPROGRESS:
            #                     print(f"Unexpected exception occurred during accept(): {e}")
            #                     raise
            
            # except select.error as e:
            #     ## 处理 select 错误...
            #     print(f"Select error occurred: {e}")
    
    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port)
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):
        self.slave =self.server.add_slave(1) 
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS, 0, 600) 
        self.slave.add_block("var", cst.COILS, 0, 600)

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)
                ### 更新机器人的状态
                status = []
                DI_status = []
                DO_status = []
                MO_status = []
                slider_status = []
                
                s = self.vsrserver.robot_control.status_global
                if s != None:
                    if s["state"] == "Idle":
                        status.append(1)
                    elif s["state"] == "Alarm":
                        status.append(2)
                    elif s["state"] == "Home":
                        status.append(3)
                    elif s["state"] == "Run":
                        status.append(4)
                    else:
                        status.append(0)
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]

                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    ### 增加机器人的类型
                    status.append(self.vsrserver.robot_control.robot_type_number)     
                    ### 将机器人的状态更新到0号寄存器
                    # self.slave.set_values("robot6", 0, status)
                    self.modbus_master.write_multiple_registers(1, 0, status)
                    config.set_value("BO0",self.vsrserver.robot_control.bo_out)
                    
                    ###DO 4个的值写入28---31寄存器地址
                    DO_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=12, quantity_of_x= 4)
                    if DO_Status is not None and len(DO_Status) == 4:
                        for i in range(len(DO_Status)):
                            DO_status.append(DO_Status[i])
                            # self.slave.set_values("robot6", 28, DO_status)
                        self.modbus_master.write_multiple_registers(1, 28, DO_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值udo
                        DO_status = [0] * 4
                        # self.slave.set_values("robot6", 28, DO_status)
                        self.modbus_master.write_multiple_registers(1, 28, DO_status)
                    
                    time.sleep(0.5)
                    
                    ###DI 12个的值写入32---43寄存器地址
                    DI_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=0, quantity_of_x= 12)

                    if DI_Status is not None and len(DI_Status) == 12:
                        for i in range(len(DI_Status)):
                            DI_status.append(DI_Status[i])
                        # self.slave.set_values("robot6", 32, DI_status)
                        self.modbus_master.write_multiple_registers(1, 32, DI_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值
                        DI_status = [0] * 12
                        # self.slave.set_values("robot6", 32, DI_status)
                        self.modbus_master.write_multiple_registers(1, 32, DI_status)
                    time.sleep(1)
                    # print(DI_status)

                    MO_Status = rtu_master.rtu_server.execute(slave=1, function_code=cst.READ_HOLDING_REGISTERS, starting_address=16, quantity_of_x= 2)
                    if MO_Status is not None and len(MO_Status) == 2:
                        for i in range(len(MO_Status)):
                            MO_status.append(MO_Status[i])
                            # self.slave.set_values("robot6", 44, MO_Status)
                        self.modbus_master.write_multiple_registers(1, 44, MO_status)
                    else:
                        # 如果获取的数据有问题，使用默认初始化值
                        MO_status = [0] * 2
                        # self.slave.set_values("robot6", 44, MO_Status)
                        self.modbus_master.write_multiple_registers(1, 44, MO_status)
            
                ### 更新机器人2的状态
                status_2 = []
                slider_status_2 = []
                s_2 = self.vsrserver.robot_control.status_global_
                if s_2 != None:
                    if s_2["state"] == "Idle":
                        status_2.append(1)
                    elif s_2["state"] == "Alarm":
                        status_2.append(2)
                    elif s_2["state"] == "Home":
                        status_2.append(3)
                    elif s_2["state"] == "Run":
                        status_2.append(4)
                    else:
                        status_2.append(0)
                    ang = s_2["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status_2.append(a[0])
                        status_2.append(a[1])

                    coor = s_2["coordinate"]
                    for i in range(len(coor)):
                        a = self.float_to_int16s(coor[i])
                        status_2.append(a[0])
                        status_2.append(a[1])
                        if i ==6:
                            slider_status_2.append(a[0])
                            slider_status_2.append(a[1])
                    
                    ### 增加机器人的类型
                    status_2.append(self.vsrserver.robot_control.robot_type_number_2)     
                    # ### 增加数字仿真服务状态
                    # status_2.append(self.vsrserver.connect_flag)
                    ### IO状态
                    ### 将2号机器人的状态更新到100号寄存器
                    # self.slave.set_values("robot6", 60, status_2)
                    self.modbus_master.write_multiple_registers(1, 60, status_2)

            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def dorun(self,):
        while True:
            try:
                time.sleep(0.1)
                # cmd_di = self.slave.get_values("robot6", 73,2)
                cmd_di = self.modbus_master.read_holding_registers(1, 73, 2)
                if cmd_di[0] == 0:
                    pass
                elif cmd_di[0] == 1:
                    self.vsrserver.robot_control.robot_stop(com_Num = 1)
                    self.vsrserver.robot_control.robot_stop(com_Num = 2)
                    # self.slave.set_values("robot6", 73, 0)
                    self.modbus_master.write_single_register(1, 73, 0)
                
                if cmd_di[1] == 0:
                    pass
                elif cmd_di[1] == 1:
                    self.vsrserver.robot_control.robot_home(com_Num=1)
                    self.vsrserver.robot_control.robot_home(com_Num=2)
                    # self.slave.set_values("robot6", 74, 0)
                    self.modbus_master.write_single_register(1, 73, 0)


                ### 3执行机器人的指令
                time.sleep(0.05)
                # cmd = self.slave.get_values("robot6", 99, 1)
                cmd = self.modbus_master.read_holding_registers(1, 99, 1)

                ### 3 Stop 1 Home 2 Zero
                if cmd[0] == 0:
                    pass
                elif cmd[0] == 1:
                    self.vsrserver.robot_control.home_position(com_Num=1)
                    self.vsrserver.robot_control.home_position(com_Num=2)
                    # self.slave.set_values("robot6", 99, 0)
                    self.modbus_master.write_single_register(1, 99, 0)
                elif cmd[0] == 2:
                    self.vsrserver.robot_control.go_to_zero()
                elif cmd[0] == 3:
                    self.vsrserver.robot_control.robot_stop(com_Num=1)
                    self.vsrserver.robot_control.robot_stop(com_Num=2)

                ### 4 控制台执行机器人的指令
                # mini_factroy_cmd = self.slave.get_values("robot6", 32, 2)
                mini_factroy_cmd = self.modbus_master.read_holding_registers(1, 32, 2)
                # mini_factroy_cmd = rtu_master.rtu_server.execute(1, cst.READ_HOLDING_REGISTERS, starting_address= 0, quantity_of_x=2)
                if mini_factroy_cmd[0] == 0 :
                    pass
                elif mini_factroy_cmd[0] == 1:
                    self.vsrserver.robot_control.robot_stop(com_Num=1)
                    self.vsrserver.robot_control.robot_stop(com_Num=2)
                
                if mini_factroy_cmd[1] == 0:
                    pass
                elif mini_factroy_cmd[1] == 1:
                    self.vsrserver.robot_control.home_position(com_Num=1)
                    self.vsrserver.robot_control.home_position(com_Num=2)
            
                ### 5Lua脚本的指令
                time.sleep(0.05)
                # cmd = self.slave.get_values("robot6", 98, 1)
                cmd = self.modbus_master.read_holding_registers(1, 98, 1)

                ### 1.runLua 2. stopLua 3. terminalLua
                if cmd[0] == 0:
                    pass
                elif cmd[0] == 1:
                    self.vsrserver.lua_robotLua.runningLua = True
                    # self.slave.set_values("robot6", 98, 0)
                    self.modbus_master.write_single_register(1, 98, 0)
                elif cmd[0] == 2:
                    self.vsrserver.robot_control.allRobot_stop()            
                    self.vsrserver.lua_robotLua.stop_process()
                    rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=12, output_value=[0,0,0,0])
                    # self.slave.set_values("robot6", 98, 0)
                    self.modbus_master.write_single_register(1, 98, 0)
                elif cmd[0] == 3:
                    self.vsrserver.robot_control.allRobot_stop()
                    self.vsrserver.lua_robotLua.stop_process()
                    rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=12, output_value=[0,0,0,0])
                    # self.slave.set_values("robot6", 98, 0)
                    self.modbus_master.write_single_register(1, 98, 0)
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)


###tcp向IOA发送数据包
class IOA_Server():
    
    def __init__(self) -> None:

        self.connect_flag = 0
        self.tcpServer()
    
    def tcpServer(self):
        self.tcp_server_socket=socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        self.tcp_server_socket.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,True)
        address2=config.get_value('address2')

        config.set_value(name="BO0",value=0)
        config.set_value(name="BO1",value=0) #初始化16个DO 
        speed_value=(self.float2bytes(75.0))
        config.set_value("FO[0]",value=speed_value)
        f01=(self.float2bytes(75.0))
        # 因为robot_control中已经有FO[1]，所以改成F0[1]
        config.set_value("F0[1]",value=f01)
        self.tcp_server_socket.bind(address2)
        self.tcp_server_socket.listen(10)
    
    #将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]      

    def checksum(self,Data,lenth):
        checksum_value=0
        data=list()
        for i in range(lenth-2):   #######从Data[2]开始，相应总长度减2
            data.append(Data[i+2])    
        for i in range(len(data)):
            checksum_value=((checksum_value+data[i]))%0xFFFF
        checksum_value_low=(checksum_value&0xFF)
        return checksum_value_low
    
    def calculate_recvdata(self,recv_value,lenth):
        
        data=list()                       
        for i in range(0,lenth):
            data.append(recv_value[i])
        # print('接收到IOA数据')
        config.set_value("BI0",data[12])    ###16位BI数据中低8位BI0----BI7                    
        config.set_value("BI1",data[13])    ###16位BI数据中高8位BI8----BI15
        ###4个一组为一个FI，共4个FI,data[17]-data[32]
        tf0=(data[17:21])
        tf1=(data[21:25])
        tf2=(data[25:29])
        tf3=(data[29:33])
        config.set_value("FI[0]",value=tf0)
        config.set_value("FI[1]",value=tf1) ##FI.append(data[17+4*i:21+4*i])
        config.set_value("FI[2]",value=tf2)
        config.set_value("FI[3]",value=tf3)  
  
    '''    ####小端字节序按位读取BI口数据
                BI0=(data[12]&0x1)
                BI1=(data[12]&0x2)>>1
                BI2=(data[12]&0x4)>>2
                BI3=(data[12]&0x8)>>3
                BI4=(data[12]&0x10)>>4
                BI5=(data[12]&0x20)>>5
                BI6=(data[12]&0x40)>>6
                BI7=(data[12]&0x80)>>7
                
                
                BI16=(data[13]&0x1)
                BI17=(data[13]&0x2)>>1
                BI18=(data[13]&0x4)>>2
                BI19=(data[13]&0x8)>>3
                BI20=(data[13]&0x10)>>4
                BI21=(data[13]&0x20)>>5
                BI22=(data[13]&0x40)>>6
                BI23=(data[13]&0x80)>>7
                BI.append(BI0,BI1)   '''
    
    def set_sendData2IOA(self):

        '''
        如果要使用数字孪生，需要根据实际来设置
        id :如果有两个控制器，另外一个要设置成2
        机器人型号：四轴是39，六轴是41
        这里需要两个参数：设置id、机器人型号
        '''
        
        data=list()
        ###帧头###
        data.extend([0xff,0xaa])        #00,01
        ###协议号###
        data.append(0x01)               #02
        ###帧长度###
        data.extend([0x63,0x00])        #03,04
        ###机器人型号###
        data.extend([0x41,0x00])        #05,06
        ###设备id号;外设类型数量###
        data.extend([0x01,0x05])        #07,08
        ###子帧（1）16个BI数据，来自IOA###
        data.append(0x01)               #09
        data.extend([0x02,0x00])        #10,11
        data.extend([config.get_value('BI0'),config.get_value('BI1')])  #12,13
        ###子帧（2）16个BO数据来自ABB ### 
        data.append(0x02)                           #14
        data.extend([0x02,0x00])                      #15,16       
        data.append(config.get_value("BO0"))        #17
        # data.append(0xff)  
        data.append(config.get_value("BO1"))        #18
        ###子帧（3）6个MS，float数据，来自ABB###
        data.append(0x03)                           #19
        data.extend([0x28,0x00])                    #20,21
        j0 = config.get_value("j0")
        j1 = config.get_value("j1")
        j2 = config.get_value("j2")
        j3 = config.get_value("j3")
        j4 = config.get_value("j4")
        j5 = config.get_value("j5")

        j6 = config.get_value("j7")
        j7 = config.get_value("j8")
        j8 = config.get_value("j9")
        j9 = config.get_value("j10")

        data.extend(self.float2bytes(j0))         #22-25
        data.extend(self.float2bytes(j1))         #26-29
        data.extend(self.float2bytes(j2))        #30-33
        data.extend(self.float2bytes(j3))         #34-37
        data.extend(self.float2bytes(j4))         #38-41
        data.extend(self.float2bytes(j5))        #42-45  

        data.extend(self.float2bytes(j6))
        data.extend(self.float2bytes(j7))         #34-37
        data.extend(self.float2bytes(j8))        #38-41
        data.extend(self.float2bytes(j9))         #42-45        
        ###子帧（4）4个FI，float数据，来自IOA ###
        data.append(0x84)                           #46
        data.extend([0x10,0x00])                    #47,48
        data.extend(config.get_value("FI[0]"))      #49-52
        data.extend(config.get_value("FI[1]"))      #53-56
        data.extend(config.get_value("FI[2]"))      #57-60
        data.extend(config.get_value("FI[3]"))      #61-64
        ###子帧（5） 4个FO数据，float，来自ABB ###
        data.append(0x85)                           #65
        data.extend([0x10,0x00])                    #66,67
        #Fo数据来自ABB示教器，暂定为0后续增加相关函数#68-83

        data.extend(config.get_value("FO[0]"))      #FO 0
        data.extend(config.get_value("F0[1]"))      #F0 1
        data_FO=[0x00]*8

        data.extend(data_FO)
        ###添加校验和###
        checksum_value=self.checksum(data,100)       #84
        data.append(checksum_value)
        ###添加帧尾###
        data.extend([0xff,0x55])                    #85,86
        return data
    
    def run(self):
        while True:
            self.connect_flag = 0
            client_socket,client_Addr=self.tcp_server_socket.accept()
            print("from:",client_Addr)
            time.sleep(0.1)
            self.connect_flag = 1 
            while True:
                try:
                    recv_data=client_socket.recv(1024)
                    if len(recv_data)<36:  ##############处理接收数据
                        # client_socket,client_Addr=self.tcp_server_socket.accept()
                        # pass
                        break
                    else:
                        for i in range(0,len(recv_data)):
                            if (recv_data[i]==0xff and recv_data[i+1]==0xaa):
                                recv_data=recv_data[i:]
                                break                  
                        if (recv_data[0]==0xff and recv_data[1]==0xaa):
                            if (recv_data[8]==0x02):
                                if (recv_data[9]==0x01 and recv_data[10]==0x02 and recv_data[14]==0x84 and recv_data[15]==0x10):
                                    self.calculate_recvdata(recv_data,36)
                                    senddata=self.set_sendData2IOA()
                                    send_data=bytes(senddata)                                    
                                    client_socket.send(send_data)                                                               
                                else:
                                    print("congfig参数不对")
                            else:
                                print("外设数目不为2")                            
                        else:
                            print('报头校验不通过')
                            
                except Exception as e:
                    robot_logger.error(e,exc_info=True)
                    break

            # client_socket.close()
                # break

        self.tcp_server_socket.close()

#### 此版本不再执行main，仅引用Controler_Server类,执行到mainwindow.py中
if __name__ == '__main__':

    # process_master = process_Master()
    rtu_master = rtu_Master()
    server = Controler_Server()
    server.dorun()



